#include "includes.h"
	.global       OSCtxSw
	.global       OSIntCtxSw
	.global       OSStartHighRdy
	.global       ARMDisableInt
	.global       ARMEnableInt
	.global       OSTickISR
	.global       cpu_sr

	
	
	.extern       OSTaskSwHook
	.extern       OSTCBHighRdy
	.extern       OSPrioHighRdy
	
	.equ MSKIRQ, 0X80
	.equ MSKFIQ, 0X60
 .equ MSKINT, 0XC0
	.equ SVCMODE, 0X13

	.section .text
	.code 32
	
ARMDisableInt:	
	STMFD	  SP!,    {R4-R12, LR}
	MRS		   R0,      CPSR
	@Set interrupt bite 
	ORR 	   R1, R0,  #MSKINT
	MSR 	   CPSR_c,  R1
	LDMFD	  SP!,    {R4-R12, PC}           

ARMEnableInt:
	STMFD	  SP!,    {R4-R12, LR}
	MSR 		  CPSR_c, R0
	LDMFD	  SP!,    {R4-R12, PC}

OSCtxSw:
	STMFD	  SP!,    {R0-R12, LR}
	MRS		   R0,     SPSR
	STMFD	  SP!,    {R0}
	MRS		   R0,     CPSR
	STMFD	  SP!,    {R0}

	@OSTCBCur->OSTCBStkPtr = Stack pointers
	LDR 		  R0,    =OSTCBCur
	LDR 		  R0,    [R0]
	STR		   SP,   [R0]
	
	BL		OSTaskSwHook

	@OSTCBCur  = OSTCBHighRdy
	LDR		   R0,   =OSTCBHighRdy
	LDR		   R1,   [R0]
	LDR		   R2,   =OSTCBCur
	STR		   R1,   [R2]
 @SP = OSTCBHighRdy->OSStk
	LDR		   SP,   [R1]

	@OSPrioCur= OSPrioHighRdy
	LDR		   R0,   =OSPrioHighRdy
	LDRB		  R0,   [R0]
	LDR	    R1,   =OSPrioCur
	STRB		  R0,   [R1]

	LDMFD	  SP!,  {R0}
 MSR     SPSR_fxsc, R0
	LDMFD	  SP!,  {R0}
	@MSR		   SPSR_cxsf, R0

	LDMFD	  SP!, {R0-R12, PC}^
	
  	
OSTickISR: 	
	BL	     OSIntEnter
	BL	     OSTimeTick
	BL	     OSIntExit
 LDR     R0,   =flag
 LDR     R0,   [R0]
 CMP     R0,   #0x1
 BEQ     OSIntCtxSw1
	LDMFD	  SP!,  {R0-R12,PC}^

OSIntCtxSw:
 LDR     R0,   =flag
 MOV     R1,   #0x01
 STR     R1,   [R0]
 MOV     PC,   LR

OSIntCtxSw1:
	BL	     OSTaskSwHook
 LDR     R0,   =flag
 MOV     R1,   #0x00
 STR     R1,   [R0]
 LDMFD	  SP!,  {R0-R12,LR}
 STMFD   SP!,  {R0-R2} 
	MOV 	   R2, 	 LR
	MRS		   R0,   CPSR
	@Change to the mode before IRQ, here change to svc mode
	MRS		   R1,   SPSR	
	ORR		   R1,   R1,#MSKIRQ
	MSR		   CPSR, R1

	STMFD	  SP!,  {R2}
	STMFD 	 SP!,  {R3-R12}
	
	@Change back to IRQ mode
	MSR		   CPSR, R0
 MRS     R0,   SPSR  @Get CPSR of svc mode
 
	@Pop R0-R2 to R3-R5
	LDMFD	  SP!,  {R3-R5}

	@Change to SVC mode	
	MSR		   CPSR, R1

	STMFD	SP!, {R3-R5}
	@Get	SPSR of SVC mode
	MRS		   R1,   SPSR
	@BIC		  R1,  #MSKIRQ
	STMFD	  SP!,  {R0-R1}
	@STMFD	  SP!,  {R2}
	
	@OSTCBCur->OSTCBStkPtr = Pointer
	LDR	 	  R1,   =OSTCBCur
	LDR		   R1,   [R1]
	STR		   SP,   [R1]


	@OSTCBCur  = OSTCBHighRdy
	LDR		   R0,   =OSTCBHighRdy
	LDR		   R1,   [R0]
	LDR		   R2,   =OSTCBCur
	STR		   R1,   [R2]
 @OSTCBHighRdy->OSTCBStkPtr = Pointer
	LDR		   SP,   [R1]

	@OSPrioCur= OSPrioHighRdy
	LDR		   R0,   =OSPrioHighRdy
	LDRB		  R0,   [R0]
	LDR		   R1,   =OSPrioCur
	STRB		  R0,   [R1]

	LDMFD 	 SP!,  {R0}
	MSR		   SPSR_fxsc, R0
	LDMFD	  SP!,  {R0}
	@MSR		   SPSR_fxsc, R0
	LDMFD	  SP!,  {R0-R12,PC}^
	
OSStartHighRdy:	
	 BL	    OSTaskSwHook
	 @OSRunning = TURE
	 LDR	   R0,   =OSRunning
	 MOV    R1,   #1	@TRUE
	 STRB   R1,   [R0]

	@SP= OSTCBHighRdy->OSTCBStkPtr
	 LDR    R0,   =OSTCBHighRdy
	 LDR    R0,   [R0]
	 LDR    SP,   [R0]

	 LDMFD  SP!,  {R0}
	 MSR 	  SPSR_fxsc, R0
	 LDMFD 	SP!,  {R0}
	 @MSR 	  SPSR_fxsc, R0
	 LDMFD 	SP!,  {R0-R12, PC}^

    .end

